//#include "math.h"
#include "app_sub.h"
#include "lib.h"
#include "user_app_sub.h"

//#define SVPWM_7

//__IO u32    u32testdisp;
//__IO float  f32testdisp;







#define SIN_COS_TABLE                                               \
{                                                                   \
    0x0000, 0x00C9, 0x0192, 0x025B, 0x0324, 0x03ED, 0x04B6, 0x057F, \
    0x0648, 0x0711, 0x07D9, 0x08A2, 0x096A, 0x0A33, 0x0AFB, 0x0BC4, \
    0x0C8C, 0x0D54, 0x0E1C, 0x0EE3, 0x0FAB, 0x1072, 0x113A, 0x1201, \
    0x12C8, 0x138F, 0x1455, 0x151C, 0x15E2, 0x16A8, 0x176E, 0x1833, \
    0x18F9, 0x19BE, 0x1A82, 0x1B47, 0x1C0B, 0x1CCF, 0x1D93, 0x1E57, \
    0x1F1A, 0x1FDD, 0x209F, 0x2161, 0x2223, 0x22E5, 0x23A6, 0x2467, \
    0x2528, 0x25E8, 0x26A8, 0x2767, 0x2826, 0x28E5, 0x29A3, 0x2A61, \
    0x2B1F, 0x2BDC, 0x2C99, 0x2D55, 0x2E11, 0x2ECC, 0x2F87, 0x3041, \
    0x30FB, 0x31B5, 0x326E, 0x3326, 0x33DF, 0x3496, 0x354D, 0x3604, \
    0x36BA, 0x376F, 0x3824, 0x38D9, 0x398C, 0x3A40, 0x3AF2, 0x3BA5, \
    0x3C56, 0x3D07, 0x3DB8, 0x3E68, 0x3F17, 0x3FC5, 0x4073, 0x4121, \
    0x41CE, 0x427A, 0x4325, 0x43D0, 0x447A, 0x4524, 0x45CD, 0x4675, \
    0x471C, 0x47C3, 0x4869, 0x490F, 0x49B4, 0x4A58, 0x4AFB, 0x4B9D, \
    0x4C3F, 0x4CE0, 0x4D81, 0x4E20, 0x4EBF, 0x4F5D, 0x4FFB, 0x5097, \
    0x5133, 0x51CE, 0x5268, 0x5302, 0x539B, 0x5432, 0x54C9, 0x5560, \
    0x55F5, 0x568A, 0x571D, 0x57B0, 0x5842, 0x58D3, 0x5964, 0x59F3, \
    0x5A82, 0x5B0F, 0x5B9C, 0x5C28, 0x5CB3, 0x5D3E, 0x5DC7, 0x5E4F, \
    0x5ED7, 0x5F5D, 0x5FE3, 0x6068, 0x60EB, 0x616E, 0x61F0, 0x6271, \
    0x62F1, 0x6370, 0x63EE, 0x646C, 0x64E8, 0x6563, 0x65DD, 0x6656, \
    0x66CF, 0x6746, 0x67BC, 0x6832, 0x68A6, 0x6919, 0x698B, 0x69FD, \
    0x6A6D, 0x6ADC, 0x6B4A, 0x6BB7, 0x6C23, 0x6C8E, 0x6CF8, 0x6D61, \
    0x6DC9, 0x6E30, 0x6E96, 0x6EFB, 0x6F5E, 0x6FC1, 0x7022, 0x7083, \
    0x70E2, 0x7140, 0x719D, 0x71F9, 0x7254, 0x72AE, 0x7307, 0x735E, \
    0x73B5, 0x740A, 0x745F, 0x74B2, 0x7504, 0x7555, 0x75A5, 0x75F3, \
    0x7641, 0x768D, 0x76D8, 0x7722, 0x776B, 0x77B3, 0x77FA, 0x783F, \
    0x7884, 0x78C7, 0x7909, 0x794A, 0x7989, 0x79C8, 0x7A05, 0x7A41, \
    0x7A7C, 0x7AB6, 0x7AEE, 0x7B26, 0x7B5C, 0x7B91, 0x7BC5, 0x7BF8, \
    0x7C29, 0x7C59, 0x7C88, 0x7CB6, 0x7CE3, 0x7D0E, 0x7D39, 0x7D62, \
    0x7D89, 0x7DB0, 0x7DD5, 0x7DFA, 0x7E1D, 0x7E3E, 0x7E5F, 0x7E7E, \
    0x7E9C, 0x7EB9, 0x7ED5, 0x7EEF, 0x7F09, 0x7F21, 0x7F37, 0x7F4D, \
    0x7F61, 0x7F74, 0x7F86, 0x7F97, 0x7FA6, 0x7FB4, 0x7FC1, 0x7FCD, \
    0x7FD8, 0x7FE1, 0x7FE9, 0x7FF0, 0x7FF5, 0x7FF9, 0x7FFD, 0x7FFE  \
    }
#define SIN_MASK 0x0300

#define U0_90       0x0200
#define U90_180     0x0300
#define U180_270    0x0000
#define U270_360    0x0100
static const s16 hSin_Cos_Table[256] = SIN_COS_TABLE;

STR_TrigComponents gS_TrigFunctions(u16 hAngle)
{

    u16 hindex;
    STR_TrigComponents Local_Components = {0};

    /* 10 bit index computation  */
    /*   hindex = (u16)(hAngle + 32768); */
    /*   hindex /= 64;     */
    hindex = (u16)(hAngle + 32768) >> 6;

    switch (hindex & SIN_MASK)
    {
    case U0_90:
        Local_Components.hSin = hSin_Cos_Table[(u8)(hindex)];
        Local_Components.hCos = hSin_Cos_Table[(u8)(0xFF - (u8)(hindex))];
        break;

    case U90_180:
        Local_Components.hSin = hSin_Cos_Table[(u8)(0xFF - (u8)(hindex))];
        Local_Components.hCos = -hSin_Cos_Table[(u8)(hindex)];
        break;

    case U180_270:
        Local_Components.hSin = -hSin_Cos_Table[(u8)(hindex)];
        Local_Components.hCos = -hSin_Cos_Table[(u8)(0xFF - (u8)(hindex))];
        break;

    case U270_360:
        Local_Components.hSin = -hSin_Cos_Table[(u8)(0xFF - (u8)(hindex))];
        Local_Components.hCos = hSin_Cos_Table[(u8)(hindex)];
        break;
    }
    return (Local_Components);
}

uint16_t Test1 = 0;
uint16_t Test2 = 0;
/**
 * @brief
 * @param
 * @return
 * @details
 * @date        VersionNumber           Modifier        ModifyContent
 *-------------------------------------------------------------------------------------
 *  2020/6/20         V1.0                 Tim           create
 */
void ADCBiasCheck(STR_Motor_Def *tS_M)
{
    static u16 s_unCnt;
    static s32 s_wBiasData[4];

    for (s_unCnt = 0; s_unCnt < 128; s_unCnt++)
    {
        if (s_unCnt == 0)
        {
            s_wBiasData[0] = 0;
            s_wBiasData[1] = 0;
            s_wBiasData[2] = 0;
            s_wBiasData[3] = 0;
        }

        REG_ADC_SW_CONV_EN();

        while (!REG_ADC_CONV_FLAG());
        REG_ADC_SQUE0_FLAG_CLR();

        s_wBiasData[AIN11] += GET_ADC1_VALUE(11);     //
        s_wBiasData[AIN12] += GET_ADC1_VALUE(12);     //
        
        s_wBiasData[AIN0] += GET_ADC1_VALUE(0);     //
        s_wBiasData[AIN9] += GET_ADC1_VALUE(9);     //

    }

    tS_M->mS_Ctr.nADCBiasData[0] = s_wBiasData[0] >> 7;
    tS_M->mS_Ctr.nADCBiasData[1] = s_wBiasData[1] >> 7;

    //+-15%, >2355, <1741
    if (tS_M->mS_Ctr.nADCBiasData[0] > 2355 || tS_M->mS_Ctr.nADCBiasData[0] < 1741) 
	{
		tS_M->eSYSPROTECT = E_OFFSET_FAULT;
		Test1++;
	}
    if (tS_M->mS_Ctr.nADCBiasData[1] > 2355 || tS_M->mS_Ctr.nADCBiasData[1] < 1741) 
	{
		tS_M->eSYSPROTECT = E_OFFSET_FAULT;
		Test2++;
	}
}

void ADCBiasCheckRun(STR_Motor_Def *tS_M)
{
    static u16 s_unCnt;
    static s32 s_wBiasData[2];

    if (tS_M->mS_Ctr.ubADCBiasState == ADC_BIASCHECK_INIT)
    {
        s_wBiasData[0] = 0;
        s_wBiasData[1] = 0;
        s_unCnt = 0;
        tS_M->mS_Ctr.ubADCBiasState = ADC_BIASCHECK_GOING;
    }
    else if (tS_M->mS_Ctr.ubADCBiasState == ADC_BIASCHECK_GOING)
    {
        s_wBiasData[0] += gS_Motor.mS_Ctr.u16ADCIaData;
        s_wBiasData[1] += gS_Motor.mS_Ctr.u16ADCIbData;
        s_unCnt++;

        if (s_unCnt == 128)
        {
            tS_M->mS_Ctr.nADCBiasData[0] = s_wBiasData[0] >> 7;
            tS_M->mS_Ctr.nADCBiasData[1] = s_wBiasData[1] >> 7;

            //+-15%, >2355, <1741
            if (tS_M->mS_Ctr.nADCBiasData[0] > 2355 || tS_M->mS_Ctr.nADCBiasData[0] < 1741) tS_M->eSYSPROTECT = E_OFFSET_FAULT;
            if (tS_M->mS_Ctr.nADCBiasData[1] > 2355 || tS_M->mS_Ctr.nADCBiasData[1] < 1741) tS_M->eSYSPROTECT = E_OFFSET_FAULT;
            tS_M->mS_Ctr.ubADCBiasState = ADC_BIASCHECK_DONE;
        }
    }

}

void FOC_GetPhaseCurrent(STR_Motor_Def *tS_M)
{
    /*get the two phase current */
    tS_M->mS_Ctr.nIa = (-tS_M->mS_Ctr.u16ADCIaData + tS_M->mS_Ctr.nADCBiasData[0]) << 4;
    tS_M->mS_Ctr.nIb = (-tS_M->mS_Ctr.u16ADCIbData + tS_M->mS_Ctr.nADCBiasData[1]) << 4;
    tS_M->mS_Ctr.nIc = -tS_M->mS_Ctr.nIa - tS_M->mS_Ctr.nIb;
}

void FOC_Coordinate_ABCtoDQ(STR_Motor_Def *tS_M, STR_TrigComponents *tS_Trig)
{
    //a-b-c to alpha-beta (clark)

    tS_M->mS_Ctr.nIalpha = tS_M->mS_Ctr.nIa;
    tS_M->mS_Ctr.nIbeta = ((int32_t)(tS_M->mS_Ctr.nIb - tS_M->mS_Ctr.nIc) * 18918) >> 15;

    //alpha-beta to d-q (park)
    tS_M->mS_Ctr.nId = ((int32_t)tS_Trig->hCos  * tS_M->mS_Ctr.nIalpha  + (int32_t)tS_Trig->hSin * tS_M->mS_Ctr.nIbeta) >> 15;
    tS_M->mS_Ctr.nIq = (-(int32_t)tS_Trig->hSin * tS_M->mS_Ctr.nIalpha  + (int32_t)tS_Trig->hCos * tS_M->mS_Ctr.nIbeta) >> 15;

}


void FOC_CurrLoop_Output(STR_Motor_Def *tS_M)
{

    if (tS_M->mS_Ctr.ubStateDiv == 1)
    {
        tS_M->mS_Ctr.nVd = wPI_controller(tS_M->mS_Ctr.nIdCmd - tS_M->mS_Ctr.nId, &gS_IdPI);
        tS_M->mS_Ctr.nVq = wPI_controller(tS_M->mS_Ctr.nIqCmd - tS_M->mS_Ctr.nIq, &gS_IqPI);
#if 1
//vbus comp
        /*VbusCompFunction(tS_M);*/
#else
        tS_M->mS_Ctr.nVqComp = tS_M->mS_Ctr.nVq;
        tS_M->mS_Ctr.nVdComp = tS_M->mS_Ctr.nVd;
#endif

#ifdef VF_OPENLOOP_MODE
        tS_M->mS_Ctr.nVdComp = 0;
        tS_M->mS_Ctr.nVqComp = 2000;
#endif
    }

    tS_M->mS_Ctr.mS_TrigSinCos = gS_TrigFunctions(tS_M->mS_Ctr.unThetaCmd);
    //inverse park
//    tS_M->mS_Ctr.nValpha = (tS_M->mS_Ctr.nVd * tS_M->mS_Ctr.mS_TrigSinCos.hCos - tS_M->mS_Ctr.nVq * tS_M->mS_Ctr.mS_TrigSinCos.hSin) >> 15;
//    tS_M->mS_Ctr.nVbeta  = (tS_M->mS_Ctr.nVd * tS_M->mS_Ctr.mS_TrigSinCos.hSin + tS_M->mS_Ctr.nVq * tS_M->mS_Ctr.mS_TrigSinCos.hCos) >> 15;
    tS_M->mS_Ctr.nValpha = (tS_M->mS_Ctr.nVdComp * tS_M->mS_Ctr.mS_TrigSinCos.hCos - tS_M->mS_Ctr.nVqComp * tS_M->mS_Ctr.mS_TrigSinCos.hSin) >> 15;
    tS_M->mS_Ctr.nVbeta  = (tS_M->mS_Ctr.nVdComp * tS_M->mS_Ctr.mS_TrigSinCos.hSin + tS_M->mS_Ctr.nVqComp * tS_M->mS_Ctr.mS_TrigSinCos.hCos) >> 15;
}

void FWREG_ParameterInit(void)
{
    gS_Motor.mS_Para.unREGPWMPeriod                 = PWM_FREQ_PERIOD_PARA;
    gS_Motor.mS_Para.unREGPWMPeriodHalf             = PWM_FREQHALF_PERIOD_PARA;
    gS_Motor.mS_Para.unREGDeadTime                  = DEADTIME_PARA;
    gS_Motor.mS_Para.unADS_SW_Trig                  = ADC_SW_TRIG_PARA;
}

void ReverseRampParaInit(void)
{
    gS_Motor.mS_Para.unSoftstopTimeTH               = SOFTSTOP_TIME_THRESHOLD_PARA;
    gS_Motor.mS_Para.unPreChargeTime                = PRECHARGE_TIME_PARA;
    gS_Motor.mS_Para.unAlign1stCurrIncrTime         = ALIGN_CURR_INCR_TIME_PARA;
    gS_Motor.mS_Para.unAlign2ndCurrIncrTime         = ALIGN_CURR_INCR_TIME_PARA;
    gS_Motor.mS_Para.unAlign1stTime                 = ALIGN_1ST_TIME_PARA;
    gS_Motor.mS_Para.unAlign2ndTime                 = ALIGN_2ND_TIME_PARA;
    gS_Motor.mS_Para.unRampStableTime               = RAMP_STABLE_TIME_PARA;
    gS_Motor.mS_Para.unAdaptiveRampStableTime       = ADAPTIVERAMP_STABLE_TIME_PARA;
    gS_Motor.mS_Para.unBrakeTime                    = BRAKE_TIME_PARA * 2;
    gS_Motor.mS_Para.unBrakeQuitSpdTimeOut          = BRAKE_TIME_PARA;//unit 1ms
    gS_Motor.mS_Para.unTransRunTime                 = RUNTRANS_TIME_PARA;


    gS_BEMFOB_PLL_PI.wUi_15                         = 0;

    /*LIB_OB_RESET_EN();
    LIB_ALM_MOTOR_LOCK_PROTECT();

    LIB_OB_RESET_EN();
    LIB_THETA_EST();*/

    gS_Motor.mS_Ctr.nIdCmd = 0;
    gS_DetBEMF.ubDetState   = 1;
    gS_Motor.mS_Ctr.ubLostPhaseCntTarget = 10;//init value for startup
    gS_Motor.mS_Ctr.ubLostPhaseCnt = 0;

    gS_BEMFOB.unThetaOut = gS_Motor.mS_Ctr.uwThetaCmdBuf >> 16;

#if 0
//adaptive ramp
    gS_Motor.mS_Ctr.nDeltaIqFactor = gS_Motor.mS_Para.nAdaptiveRampAccCurr;
    gS_Motor.mS_Ctr.nIqCmdReq      = gS_Motor.mS_Para.nAdaptiveRampCurrCmdReq;
    LIB_ADAPTIVERAMP_INIT();
    gS_Motor.eFSM = E_FSM_SYS_ADAPTIVERAMP;
#else
//sync ramp
    gS_Motor.mS_Ctr.nDeltaWrFactor = gS_Motor.mS_Para.nRampDeltaWrFactor;
    if (gS_Motor.eDIR_CMD == E_CW)
        gS_Motor.mS_Ctr.vnSpdCmdReq    = gS_Motor.mS_Para.nRampSpdCmdReq;
    else
        gS_Motor.mS_Ctr.vnSpdCmdReq    = -gS_Motor.mS_Para.nRampSpdCmdReq;
    gS_Motor.mS_Ctr.vnSpdCmd       = 0;
    gS_Motor.mS_Ctr.nDeltaIdFactor = gS_Motor.mS_Para.nRampAccCurr;
    gS_Motor.mS_Ctr.nIqCmdReq      = gS_Motor.mS_Para.nRampCurrCmdReq;
    gS_Motor.mS_Ctr.nDeltaIqFactor = gS_Motor.mS_Para.nRampAccCurr;
    gS_Motor.eFSM = E_FSM_SYS_RAMP;
#endif
}

void ParameterInit(void)
{
    gS_Motor.eDIR_CMD                               = CMD_DIR;
    gS_Motor.mS_PU.nPhaseCurrPU                     = PU_CURR_PARA;
    gS_Motor.mS_PU.nPhaseVoltPU                     = PU_VOLT_PARA;
    gS_Motor.mS_PU.fSpdPU                           = PU_SPD_PARA;
    gS_Motor.mS_PU.fDispMaxCurr                     = DISP_MAX_CURR_PARA;
    gS_Motor.mS_PU.fDispMaxVbus                     = DISP_MAX_VBUS_PARA;
    gS_Motor.mS_PU.wDispMaxSpdRPM                   = DISP_MAX_SPD_PARA;

    gS_Motor.mS_Para.unVbusOVPLevel                 = ALM_OVP_LEVEL_PARA;
    gS_Motor.mS_Para.unVbusOVPReleaseLevel          = ALM_OVP_RELEASE_LEVEL_PARA;
    gS_Motor.mS_Para.unVbusUVPLevel                 = ALM_UVP_LEVEL_PARA;
    gS_Motor.mS_Para.unVbusUVPReleaseLevel          = ALM_UVP_RELEASE_LEVEL_PARA;

    gS_Motor.mS_Para.ubOCPTimesTarget               = ALM_OCP_TIMES_PARA;
    gS_Motor.mS_Para.nOCPLevel                      = ALM_OCP_LEVEL_PARA;

    gS_Motor.mS_Para.unRampTimeoutTarget            = ALM_RAMP_TIMEOUT_PARA;
    gS_Motor.mS_Para.ubLostPhaseCntTimes            = ALM_LOSTPHASE_TIMES_PARA;

    gS_Motor.mS_Para.unSoftstopTimeTH               = SOFTSTOP_TIME_THRESHOLD_PARA;
    gS_Motor.mS_Para.nSoftstopSpdTH                 = SOFTSTOP_SPD_THRESHOLD_PARA;
    gS_Motor.mS_Para.nSoftstopIdCmd                 = SOFTSTOP_ID_CMD_PARA;
    gS_Motor.mS_Para.nSoftstopIqCmd                 = SOFTSTOP_IQ_CMD_PARA;

    gS_Motor.mS_Para.unPreChargeTime                = PRECHARGE_TIME_PARA;
    gS_Motor.mS_Para.unPreChargeDuty                = PRECHARGE_DUTY_PARA;
    gS_Motor.mS_Para.unPWMIRQ1kHzCntTarget          = PWM_IRQ_1KHZ_CNT_PARA;
    gS_Motor.mS_Para.unAlign1stCurrIncrTime         = ALIGN_CURR_INCR_TIME_PARA;
    gS_Motor.mS_Para.unAlign2ndCurrIncrTime         = ALIGN_CURR_INCR_TIME_PARA;
    gS_Motor.mS_Para.nAlign1stCurr                  = ALIGN_1ST_CURR_PARA;
    gS_Motor.mS_Para.nAlign2ndCurr                  = ALIGN_2ND_CURR_PARA;
    gS_Motor.mS_Para.unAlign1stTime                 = ALIGN_1ST_TIME_PARA;
    gS_Motor.mS_Para.unAlign2ndTime                 = ALIGN_2ND_TIME_PARA;
    gS_Motor.mS_Para.nAlign1stAccCurr               = ALIGN_1ST_CURR_ACC_PARA;
    gS_Motor.mS_Para.nAlign2ndAccCurr               = ALIGN_2ND_CURR_ACC_PARA;

    gS_Motor.mS_Para.unDetTimeOutCmd                = BEMFDET_DET_TIMEOUT_PARA;
    gS_Motor.mS_Para.unDetHighSpdTOCntCmd           = BEMFDET_DET_TIMEOUT_PARA >> 1;
    gS_Motor.mS_Para.unDetSpdDetLimitMin            = BEMFDET_SPD_DET_MIN_PARA;
    gS_Motor.mS_Para.unDetSpdDetLimitMax            = BEMFDET_SPD_DET_MAX_PARA;
    gS_Motor.mS_Para.unDetConectFactor              = BEMFDET_CONECT_FACTOR_PARA;
    gS_Motor.mS_Para.unDetReverseSpdTH              = REVERSE_SPD_TH_PARA;
    gS_Motor.mS_Para.nDetReverseIdCmd               = REVERSE_ID_CMD_PARA;
    gS_Motor.mS_Para.nDetReverseIqCmd               = REVERSE_IQ_CMD_PARA;

//#if(RAMP_MODE == SYNC_RAMP)
    gS_Motor.mS_Para.nRampDeltaWrFactor             = RAMP_DELTA_WR_FACTOR_PARA;
    gS_Motor.mS_Para.nRampSpdCmdReq                 = RAMP_SPD_TARGET_PARA;
    gS_Motor.mS_Para.nRampCurrCmdReq                = RAMP_CURR_PARA;
    gS_Motor.mS_Para.nRampAccCurr                   = RAMP_CURR_ACC_PARA;
    gS_Motor.mS_Para.unRampStableTime               = RAMP_STABLE_TIME_PARA;
//#endif
    gS_Motor.mS_Para.nReverseRampDeltaWrFactor      = REVERSERAMP_DELTA_WR_FACTOR_PARA;
    gS_Motor.mS_Para.nReverseRampCurrCmdReq         = REVERSERAMP_CURR_PARA;

#if(RAMP_MODE == ADAPTIVE_RAMP)
    gS_Motor.mS_Para.nAdaptiveRampCurrCmdReq        = ADAPTIVERAMP_CURR_PARA;
    gS_Motor.mS_Para.nAdaptiveRampAccCurr           = ADAPTIVERAMP_CURR_ACC_PARA;
    gS_Motor.mS_Para.unAdaptiveRampStableTime       = ADAPTIVERAMP_STABLE_TIME_PARA;
    gS_Motor.mS_Para.nAdaptiveRampSpdTarget         = ADAPTIVERAMP_SPD_TARGET_PARA;
    gS_Motor.mS_Para.nAdaptiveRampInitDeltaTheta    = ADAPTIVERAMP_DELTATHETA_PARA;
    gS_Motor.mS_Para.nAdaptiveRampMinDeltaTheta     = ADAPTIVERAMP_MIN_DELTATHETA_PARA;

#endif
    gS_Motor.mS_Para.nVbusCompGain                  = VBUSCOMPGAIN_PARA;
    gS_Motor.mS_Para.nRunDeltaWrFactor              = RUN_DELTA_WR_FACTOR_PARA;

    gS_Motor.mS_Para.nSpdToThetaGain                = SPD_TO_THETA_GAIN_PARA;
    gS_Motor.mS_Para.nTwoShuntSampleWindow          = TWO_SHUNT_SAMPLE_WINDOW_PARA;
    gS_Motor.mS_Para.nVsMax                         = VS_MAX_PARA;
    gS_Motor.mS_Para.wVsMaxSqar                     = VS_MAX_PARA * VS_MAX_PARA;
    gS_Motor.mS_Para.nVsMaxSqarQ                    = VS_MAX_SQARQ_PARA;
    gS_Motor.mS_Para.nTsMax                         = TS_MAX_PARA;
    gS_Motor.mS_PU.fDispMaxDuty                     = DISP_MAX_DUTY_PARA;

    gS_Motor.mS_Para.nOverSpdLevelH                 = ALM_OVERSPD_LEVEL_H_PARA;
    gS_Motor.mS_Para.nOverSpdLevelL                 = ALM_OVERSPD_LEVEL_L_PARA;
    gS_Motor.mS_Para.unOverSpdTime                  = ALM_OVERSPD_TIME_PARA;
    gS_Motor.mS_Para.unLostSYNCTime                 = ALM_LOSTSYNC_TIME_PARA;
    gS_Motor.mS_Para.unLostSYNCSpdFactor            = ALM_LOSTSYNC_SPDFACTOR_PARA;

    gS_Motor.mS_Para.nVbusCompGain                  = VBUSCOMPGAIN_PARA;


    gS_Motor.mS_Para.E_SVM_MODE_PARA                = SVM_MODE_PARA;
    gS_SVM.E_SVM_MODE                               = gS_Motor.mS_Para.E_SVM_MODE_PARA;
    gS_SVM.nMaxDuty                                 = gS_Motor.mS_Para.nTsMax;

    gS_BMEFSense.unBemfStopTH                       = BEMF_STOP_TH_PARA;
    gS_BMEFSense.unBemfCloseLoopTH                  = BEMF_CLOSELOOP_TH_PARA;
    gS_BMEFSense.uwBemfVGain                        = BEMF_V_GAIN_PARA;
    gS_Motor.mS_Para.unBrakeTime                    = BRAKE_TIME_PARA * 2;
    gS_Motor.mS_Para.unBrakeQuitSpdTimeOut          = BRAKE_TIME_PARA;//unit 1ms
    gS_Motor.mS_Para.unBrakeQuitSpd                 = BRAKE_QUIT_SPD_PARA;
    gS_Motor.mS_Para.unBrakeQuitSpdPU               = BRAKE_QUIT_SPD_PU_PARA;
    gS_Motor.mS_Para.unTransRunTime                 = RUNTRANS_TIME_PARA;

#if(P_LOOP_MODE == P_LOOP_SPD)
    gS_Motor.mS_Ctr.nCmdReqMax                      = CMD_MAX_VALUE_SPD_PARA;
    gS_Motor.mS_Ctr.nCmdReqMin                      = CMD_MIN_VALUE_SPD_PARA;
    gS_VSP.unStartLevel                             = P_VSP_STARTLEVEL_PARA;
    gS_VSP.unStopLevel                              = P_VSP_STOPLEVEL_PARA;
    gS_VSP.unFactor                                 = P_SPD_VSP_FACTOR_PARA;
#elif(P_LOOP_MODE == P_LOOP_POWER)
    gS_Motor.mS_Ctr.nCmdReqMax                      = P_POWER_CMD_MAX_VALUE_PARA;
    gS_Motor.mS_Ctr.nCmdReqMin                      = P_POWER_CMD_MIN_VALUE_PARA;
    gS_VSP.unStartLevel                             = P_VSP_STARTLEVEL_PARA;
    gS_VSP.unStopLevel                              = P_VSP_STOPLEVEL_PARA;
    gS_VSP.unFactor                                 = P_VSP_FACTOR_PARA;
#endif

    gS_Motor.mS_Para.nDeadTimeCompValue             = DEADTIME_COMP_VALUE_PARA;

    gS_SMO.wF                                       = SMO_F_PARA;
    gS_SMO.wG                                       = SMO_G_PARA;
    gS_SMO.nKslide                                  = SMO_KSLIDE_PARA;
    gS_SMO.nE0                                      = SMO_E0_PARA;
    gS_SMO.nKslf                                    = SMO_KSLF_PARA;
    gS_SMO.nWeLowpass                               = SMO_WE_LOWPASS_PARA;
    gS_SMO.nSpdGainInteger                          = SMO_SPDGAIN_INTEGER_PARA;
    gS_SMO.nSpdGainDecimal                          = SMO_SPDGAIN_DECIMAL_PARA;

    gS_PLL_PI.wKp_15                                = SMO_PLL_KP_PARA;
    gS_PLL_PI.wKi_15                                = SMO_PLL_KI_PARA;
    gS_PLL_PI.wOutlimitmax                          = SMO_PLL_OUT_MAX_PARA;
    gS_PLL_PI.wOutlimitmin                          = SMO_PLL_OUT_MIN_PARA;
    gS_PLL_PI.wUilimitmax                           = gS_PLL_PI.wOutlimitmax << 15;
    gS_PLL_PI.wUilimitmin                           = gS_PLL_PI.wOutlimitmin << 15;
    gS_PLL_PI.wUi_15                                = 0;

    gS_IqPI.wKp_15                                  = IQ_KP_PARA;
    gS_IqPI.wKi_15                                  = IQ_KI_PARA;
    gS_IqPI.wOutlimitmax                            = VQMAX_PARA;
    gS_IqPI.wOutlimitmin                            = VQMIN_PARA;
    gS_IqPI.wUilimitmax                             = gS_IqPI.wOutlimitmax << 15;
    gS_IqPI.wUilimitmin                             = gS_IqPI.wOutlimitmin << 15;
    gS_IqPI.wUi_15                                  = 0;

    gS_IdPI.wKp_15                                  = ID_KP_PARA;
    gS_IdPI.wKi_15                                  = ID_KI_PARA;
    gS_IdPI.wOutlimitmax                            = VDMAX_PARA;
    gS_IdPI.wOutlimitmin                            = VDMIN_PARA;
    gS_IdPI.wUilimitmax                             = gS_IqPI.wOutlimitmax << 15;
    gS_IdPI.wUilimitmin                             = gS_IqPI.wOutlimitmin << 15;
    gS_IdPI.wUi_15                                  = 0;

    gS_SPD_PI.wKp_15                                = SPD_KP_PARA;
    gS_SPD_PI.wKi_15                                = SPD_KI_PARA;
    gS_SPD_PI.wOutlimitmax                          = SPD_PI_OUTPUT_MAX_PARA;
    gS_SPD_PI.wOutlimitmin                          = SPD_PI_OUTPUT_MIN_PARA;
    gS_SPD_PI.wUilimitmax                           = gS_SPD_PI.wOutlimitmax << 15;
    gS_SPD_PI.wUilimitmin                           = gS_SPD_PI.wOutlimitmin << 15;
    gS_SPD_PI.wUi_15                                = 0;


    gS_BEMFOB.unGainR                               = BEMFOB_GAIN_R_PARA;
    gS_BEMFOB.unGainL                               = BEMFOB_GAIN_L_PARA;
    gS_BEMFOB.unGainW                               = BEMFOB_GAIN_W_PARA;
    gS_BEMFOB.nWeLowpass                            = BEMFOB_WE_LOWPASS_PARA;
    gS_BEMFOB.nSpdGainInteger                       = BEMFOB_SPDGAIN_INTEGER_PARA;
    gS_BEMFOB.nSpdGainDecimal                       = BEMFOB_SPDGAIN_DECIMAL_PARA;

    gS_BEMFOB_PLL_PI.wKp_15                         = BEMFOB_PLL_KP_PARA;
    gS_BEMFOB_PLL_PI.wKi_15                         = BEMFOB_PLL_KI_PARA;
    gS_BEMFOB_PLL_PI.wOutlimitmax                   = BEMFOB_PLL_OUT_MAX_PARA;
    gS_BEMFOB_PLL_PI.wOutlimitmin                   = BEMFOB_PLL_OUT_MIN_PARA;
    gS_BEMFOB_PLL_PI.wUilimitmax                    = gS_BEMFOB_PLL_PI.wOutlimitmax << 15;
    gS_BEMFOB_PLL_PI.wUilimitmin                    = gS_BEMFOB_PLL_PI.wOutlimitmin << 15;
    gS_BEMFOB_PLL_PI.wUi_15                         = 0;

    /*LIB_OB_RESET_EN();
    LIB_ALM_MOTOR_LOCK_PROTECT();

    LIB_OB_RESET_EN();
    LIB_THETA_EST();*/


    gS_Motor.mS_Ctr.nIdCmd = 0;
    gS_Motor.mS_Ctr.nIqCmd = 0;
    gS_Motor.mS_Ctr.vnSpdCmd = 0;
    gS_Motor.mS_PU.wDispSpdCmdRPM = 0;
    gS_Motor.mS_PU.wDispSpdInfoRPM = 0;

    gS_BMEFSense.nWe = 0;
    gS_BMEFSense.nDirCnt = 0;
    gS_BMEFSense.unBemfPeak = 0;
    gS_BMEFSense.unBemfWeakCnt = 0;


    gS_DetBEMF.ubDetState   = 1;

    gS_Motor.mS_Ctr.ubLostPhaseCntTarget = 10;//init value for startup
    gS_Motor.mS_Ctr.ubLostPhaseCnt = 0;

    gS_IPD.ubVarReset = ENABLE;
    /*IPD(&gS_IPD, &gS_Motor);*/

#if(P_CONTROL_MODE == P_CONTROL_DUTY)
    LPFs LPF_1 = {0, 0};
#endif

#if(P_LOOP_MODE == P_LOOP_POWER)
    gS_Power_PI.wKp_15                                = P_POWER_KP_PARA;
    gS_Power_PI.wKi_15                                = P_POWER_KI_PARA;
    gS_Power_PI.wOutlimitmax                          = P_POWER_PI_MAX_SPD_PARA * PU_SPD_PARA;
    gS_Power_PI.wOutlimitmin                          = P_POWER_PI_MIN_SPD_PARA * PU_SPD_PARA;
    gS_Power_PI.wUilimitmax                           = gS_SPD_PI.wOutlimitmax << 15;
    gS_Power_PI.wUilimitmin                           = gS_SPD_PI.wOutlimitmin << 15;
    gS_Power_PI.wUi_15                                = 0;
#endif


    gS_Motor.mS_Ctr.ubBrakeVarReset = ENABLE;
    /*ubSpd_Det(&gS_Motor);*/

    gS_Motor.mS_Ctr.ubADCBiasState = ADC_BIASCHECK_INIT;

    gS_Motor.eFSM           = E_FSM_SYS_STANDBY;
}

void Spd_ACC(STR_Motor_Def *tS_M)
{
    if (tS_M->mS_Ctr.vnSpdCmdReq > tS_M->mS_Ctr.vnSpdCmd)
    {
        tS_M->mS_Ctr.vnSpdCmd += tS_M->mS_Ctr.nDeltaWrFactor;
        if (tS_M->mS_Ctr.vnSpdCmd > tS_M->mS_Ctr.vnSpdCmdReq) tS_M->mS_Ctr.vnSpdCmd = tS_M->mS_Ctr.vnSpdCmdReq;
    }
    else if (tS_M->mS_Ctr.vnSpdCmdReq < tS_M->mS_Ctr.vnSpdCmd)
    {
        tS_M->mS_Ctr.vnSpdCmd -= tS_M->mS_Ctr.nDeltaWrFactor;
        if (tS_M->mS_Ctr.vnSpdCmd < tS_M->mS_Ctr.vnSpdCmdReq) tS_M->mS_Ctr.vnSpdCmd = tS_M->mS_Ctr.vnSpdCmdReq;
    }
}

void Iq_ACC(STR_Motor_Def *tS_M)
{
    if (tS_M->mS_Ctr.nIqCmdReq > tS_M->mS_Ctr.nIqCmd)
    {
        tS_M->mS_Ctr.nIqCmd += tS_M->mS_Ctr.nDeltaIqFactor;
        if (tS_M->mS_Ctr.nIqCmd > tS_M->mS_Ctr.nIqCmdReq) tS_M->mS_Ctr.nIqCmd = tS_M->mS_Ctr.nIqCmdReq;
    }
    else if (tS_M->mS_Ctr.nIqCmdReq < tS_M->mS_Ctr.nIqCmd)
    {
        tS_M->mS_Ctr.nIqCmd -= tS_M->mS_Ctr.nDeltaIqFactor;
        if (tS_M->mS_Ctr.nIqCmd < tS_M->mS_Ctr.nIqCmdReq) tS_M->mS_Ctr.nIqCmd = tS_M->mS_Ctr.nIqCmdReq;
    }
}


void Id_ACC(STR_Motor_Def *tS_M)
{
    if (tS_M->mS_Ctr.nIdCmdReq > tS_M->mS_Ctr.nIdCmd)
    {
        tS_M->mS_Ctr.nIdCmd += tS_M->mS_Ctr.nDeltaIdFactor;
        if (tS_M->mS_Ctr.nIdCmd > tS_M->mS_Ctr.nIdCmdReq) tS_M->mS_Ctr.nIdCmd = tS_M->mS_Ctr.nIdCmdReq;
    }
    else if (tS_M->mS_Ctr.nIdCmdReq < tS_M->mS_Ctr.nIdCmd)
    {
        tS_M->mS_Ctr.nIdCmd -= tS_M->mS_Ctr.nDeltaIdFactor;
        if (tS_M->mS_Ctr.nIdCmd < tS_M->mS_Ctr.nIdCmdReq) tS_M->mS_Ctr.nIdCmd = tS_M->mS_Ctr.nIdCmdReq;
    }
}

void SPD_Loop_Ctr(STR_Motor_Def *tS_M)
{
    s16 t_EstSpd = 0;

    t_EstSpd = LIB_OB_MOTOR_SPD;

    if (tS_M->eDIR_CMD == E_CW)
    {
        tS_M->mS_Ctr.nIqCmd = wPI_controller(tS_M->mS_Ctr.vnSpdCmd - t_EstSpd, &gS_SPD_PI);
    }
    else
    {
        tS_M->mS_Ctr.nIqCmd = wPI_controller(-tS_M->mS_Ctr.vnSpdCmd + t_EstSpd, &gS_SPD_PI);
    }
}


s32 wPI_controller(s16 t_nError, STR_PIC *tS_C)
{
    int32_t t_wTemp = 0;


    tS_C->wUi_15 += ((int32_t)(tS_C->wKi_15 * tS_C->nErr));

    if (tS_C->wUi_15 >= (tS_C->wUilimitmax))  tS_C->wUi_15 = tS_C->wUilimitmax;
    else if (tS_C->wUi_15 <= (tS_C->wUilimitmin))  tS_C->wUi_15 = tS_C->wUilimitmin;

    tS_C->nErr = t_nError;

    t_wTemp = ((tS_C->wKp_15 * t_nError) >> 15) + ((tS_C->wUi_15 >> 15));

    if (t_wTemp >= tS_C->wOutlimitmax)       t_wTemp = tS_C->wOutlimitmax;
    else if (t_wTemp <= tS_C->wOutlimitmin)  t_wTemp = tS_C->wOutlimitmin;

    return t_wTemp;//(int32)
}

void ALM_OVP_Recover(void)
{
    if (gS_Motor.mS_Ctr.unVbusFilter > gS_Motor.mS_Para.unVbusOVPLevel)
    {
        gS_Motor.eSYSPROTECT = E_OVP;
    }
    else if (gS_Motor.mS_Ctr.unVbusFilter < gS_Motor.mS_Para.unVbusOVPReleaseLevel && gS_Motor.eSYSPROTECT == E_OVP)
    {
        if (gS_Motor.eSYSPROTECT != E_OFFSET_FAULT)
            gS_Motor.eSYSPROTECT = E_NONEFAULT;

        gS_Motor.eFSM = E_FSM_SYS_STOP;
    }
}

void ALM_UVP_Recover(void)
{
    if (gS_Motor.mS_Ctr.unVbusFilter < gS_Motor.mS_Para.unVbusUVPLevel)
    {
        gS_Motor.eSYSPROTECT = E_UVP;
    }
    else if (gS_Motor.mS_Ctr.unVbusFilter > gS_Motor.mS_Para.unVbusUVPReleaseLevel && gS_Motor.eSYSPROTECT == E_UVP)
    {
        if (gS_Motor.eSYSPROTECT != E_OFFSET_FAULT)
            gS_Motor.eSYSPROTECT = E_NONEFAULT;

        gS_Motor.eFSM = E_FSM_SYS_STOP;
    }
}

void ALM_OCP(void)
{
    if (gS_Motor.mS_Ctr.nIa > gS_Motor.mS_Para.nOCPLevel || gS_Motor.mS_Ctr.nIa < -gS_Motor.mS_Para.nOCPLevel)
    {
        gS_Motor.mS_Ctr.ubOCPTimes++;
    }
    else if (gS_Motor.mS_Ctr.nIb > gS_Motor.mS_Para.nOCPLevel || gS_Motor.mS_Ctr.nIb < -gS_Motor.mS_Para.nOCPLevel)
    {
        gS_Motor.mS_Ctr.ubOCPTimes++;
    }

    else if (gS_Motor.mS_Ctr.nIc > gS_Motor.mS_Para.nOCPLevel || gS_Motor.mS_Ctr.nIc < -gS_Motor.mS_Para.nOCPLevel)
    {
        gS_Motor.mS_Ctr.ubOCPTimes++;
    }
    else if (gS_Motor.mS_Ctr.ubOCPTimes > 0)
    {
        gS_Motor.mS_Ctr.ubOCPTimes--;
    }

    if (gS_Motor.mS_Ctr.ubOCPTimes >= gS_Motor.mS_Para.ubOCPTimesTarget)
    {
        gS_Motor.eSYSPROTECT = E_OCP;
        REG_PWM_Out_Disable();
    }

}

void ALM_RAMPTimeout(void)
{
    static u16 s_unTimeoutCnt = 0;


    if (gS_Motor.eFSM == E_FSM_SYS_ADAPTIVERAMP)
    {
        if (s_unTimeoutCnt++ >= gS_Motor.mS_Para.unRampTimeoutTarget)
        {
            gS_Motor.eSYSPROTECT = E_RAMP_OVERTIME;
        }
    }
    else s_unTimeoutCnt = 0;
}

void Spd_Disp(STR_Motor_Def *tS_M)
{
    s16 t_EstSpd = 0;

    t_EstSpd = LIB_OB_MOTOR_SPD;

    tS_M->mS_PU.wDispSpdCmdRPM = (tS_M->mS_Ctr.vnSpdCmd * tS_M->mS_PU.wDispMaxSpdRPM) >> 15;
    tS_M->mS_PU.wDispSpdInfoRPM = (t_EstSpd * tS_M->mS_PU.wDispMaxSpdRPM) >> 15;
}

